#include "LdPcl.h"
int main()
{
    std::vector<std::string> BroadCode;
    BroadCode.push_back("000000000000001");
    PcloudI cloud_in(new CloudI());
    PcloudI cloud_out(new CloudI());
    PcloudI All_cloud(new CloudI());
    PcloudI All_cloud_(new CloudI());
    // Fill in the CloudIn data
    LdsLidar &mylidar = LdsLidar::GetInstance();
    int init = mylidar.InitLdsLidar(BroadCode, 40000);
    sleep(1);
    while (!init)
    {
        init = mylidar.InitLdsLidar(BroadCode, 40000);
        sleep(0.1);
    }
    bool sec = false;
    while (true)
    {
        if (!sec)
        {
            PcloudI temp(new CloudI());
            mylidar.getcloud(temp);
            while (temp == nullptr || temp->size() < 30000)
                {
                    sleep(0.01);
                    mylidar.getcloud(temp);
                }
            sec = true;
            Filter(temp, cloud_in);
            *All_cloud = *cloud_in;
            *All_cloud_ = *cloud_in;
        }
        else
        {
            PcloudI temp(new CloudI());
            mylidar.getcloud(temp);
            while (temp == nullptr || temp->size() < 30000)
                mylidar.getcloud(temp);
            Filter(temp, cloud_out);
            Fusion(cloud_out, All_cloud_);
            pcl::visualization::CloudViewer V_duizhao("V_Duizhao");
            V_duizhao.showCloud(All_cloud_);
            while(!V_duizhao.wasStopped()){}
            // pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
            pcl::IterativeClosestPoint<PointI, PointI> icp;
            icp.setInputSource(cloud_out);
            icp.setInputTarget(cloud_in);
            // icp.
            icp.setMaxCorrespondenceDistance(1.f);
            icp.setRANSACIterations(100000);
            PcloudI Final(new CloudI());
            icp.align(*Final);
            std::cout << "Final's points :" << Final->points.size() << std::endl;
            pcl::visualization::CloudViewer V_final("Viewer Final");
            V_final.showCloud(Final);
            while(!V_final.wasStopped()){}

            std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
            std::cout << icp.getFinalTransformation() << std::endl;
            Eigen::Matrix4f RoT = icp.getFinalTransformation();
            
            Fusion(cloud_out, All_cloud, RoT);
            sec = true;
        }
        pcl::visualization::CloudViewer V_All("AllClouds Viewer");
        V_All.showCloud(All_cloud);
        while (!V_All.wasStopped())
        {
        }
    }
    return (0);
}